//
//  OGLsensor.m
//  CommandCenter08
//
//  Created by Matt Roman on 7/20/08.
//  Copyright 2008 University of Oklahoma. All rights reserved.
//

#import "OGLsensor.h"

#define FONTSCALE	0.2

//takes an array of at least PROFILESEG in length and fills the first PRFOFILESEG elements with the heights of the surface
void getProfileHeights(laserScanner *lazer, float heights[])
{
	int i;
	for(i=0;i<rk.PROFILESEG;i++)
		heights[i] = rk.PROFILEHEIGHT - cos(i*rk.PROFILEINCR)*(float)lazer->rangeData[i]/1000.0;	
}

//takes an array of at least PANELSEG and fills it with the heights from a flat surface.  Note that since the measurements
//are the shortest possible ray paths, the two center segments are calculated as if they are both at an angle 0
//also note that the panel scanner is mounted at 45 degrees, hence the SQRTTWO fact in the height calculation
void getPanelHeights(laserScanner *lazer, float heights[])
{
	int i,j;
	for(i=0;i<rk.PANELSEGMENTS/2;i++){
		j=1+i-rk.PANELSEGMENTS/2;
		heights[i] = rk.PANELHEIGHT - cos(j*rk.PANELINCR)*((float)lazer->rangeData[i])/(SQRTTWO*1000.0);	
	}
	for(i=rk.PANELSEGMENTS/2;i<rk.PANELSEGMENTS;i++){
		j= rk.PANELSEGMENTS/2 - i;
		heights[i] = rk.PANELHEIGHT - cos(j*rk.PANELINCR)*((float)lazer->rangeData[i])/(SQRTTWO*1000.0);	
	}
}


@implementation OGLsensor
// pixel format definition
+ (NSOpenGLPixelFormat*) basicPixelFormat
{
    NSOpenGLPixelFormatAttribute attributes [] = {
        NSOpenGLPFAWindow,
        NSOpenGLPFADoubleBuffer,	// double buffered
        NSOpenGLPFADepthSize, (NSOpenGLPixelFormatAttribute)16, // 16 bit depth buffer
        (NSOpenGLPixelFormatAttribute)nil
    };
    return [[[NSOpenGLPixelFormat alloc] initWithAttributes:attributes] autorelease];
}

- (id)initWithFrame:(NSRect)frameRect andRover:(Rover *)r sensorType:(SenseType)t
{
	NSOpenGLPixelFormat *pf = [OGLsensor basicPixelFormat];
	self = [super initWithFrame:frameRect pixelFormat:pf];
	bot = r;
	type = t;
	heights = NULL;
	heightPosition = NULL;
	[name setFrameRotation:90];
	if(type == SenseBodyLaser){
		scanner = [bot bodyLaser];
		displayAngle = 0;
		scale = 1;
	}
	else if(type == SensePanelLaser || type == SensePanelHeight) {
		scanner = [bot panelLaser];
		displayAngle = 0;
		scale = 2.5;
		if(type == SensePanelHeight)[self refresh];
	}
	else if(type == SenseProfileLaser || type == SenseProfileHeight){
		scanner = [bot profileLaser];
		displayAngle = 180;
		scale = 2.5;
		if(type == SenseProfileHeight)[self refresh];
	}
	return self;
}

- (void)prepareOpenGL
{
	GLint swapInt = 1;
	//set to vbl sync
	[[self openGLContext] setValues:&swapInt forParameter:NSOpenGLCPSwapInterval];
	//init GL stuff here
	glClearDepth(1.0f); 
	glDepthFunc(GL_LESS);
	glEnable(GL_DEPTH_TEST);
	glShadeModel(GL_SMOOTH);
	glClearColor(0,0,0,0);
	glViewport(0, 0, [self frame].size.width, [self frame].size.height);
	// set projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(55.0, 1.0, 0.2, 5.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

- (void)drawRect:(NSRect) bounds
{
	glLoadIdentity();
	 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	gluLookAt(0, 0, 2, 0, 0, 0, 0, 1, 0);
	if(type == SenseCompass) [self drawArrow];
	else if(type == SenseBodyLaser || type == SenseProfileLaser || type == SensePanelLaser) [self drawFan];
	else if(type == SensePanelHeight) [self drawLineProfile];
	else if(type == SenseProfileHeight) [self drawBars];
	else{
		glClearColor(0, 0, 0, 0);
		glClear(GL_COLOR_BUFFER_BIT);
	}
	[[self openGLContext] flushBuffer];
}

- (void) drawArrow
{
	static float elev;
	static int alarm = 0;
	if(alarm == 0) glClearColor(0, 0, 0, 0);
	else glClearColor(1,0,0,0);
	
	if([bot getRobotProperties]->tilt == 0) elev = 0;
	else elev = [bot getRobotProperties]->tilt/45.0;

	glPushMatrix();
	glTranslatef(0.0,elev,0.0);
	glRotatef([bot getRobotProperties]->roll,0.0,0.0,1.0);
	glColor3f(1.0f,1.0f,1.0f);
	glLineWidth(10.0);
	glBegin(GL_LINES);
	{
		glVertex3f(-3.0,0.0, 0.0);
		glVertex3f(3.0,0.0, 0.0);
	}
	glEnd();
	glPopMatrix();
	glPushMatrix();
	glRotatef(360.0-[bot getRobotProperties]->compass,0.0,0.0,1.0);
    glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_TRIANGLES);
	{
		glVertex3f(  0.0,  0.7, 0.0);
		glVertex3f( -0.1, 0.0, 0.0);
		glVertex3f(  0.1, 0.0 ,0.0);
		glColor3f(1.0f, 1.0f, 1.0f);
		glVertex3f(  0.0,  -0.7, 0.0);
		glVertex3f( -0.1, 0.0, 0.0);
		glVertex3f(  0.1, 0.0 ,0.0);
	}
	glEnd();
	glLineWidth(4.0);
	glBegin(GL_LINE_STRIP);
	{
		glVertex3f( -0.1, 0.0, 0.0);
		glVertex3f(  0.0,  0.7, 0.0);
		glVertex3f(  0.1, 0.0 ,0.0);
	}
	glEnd();
	glPopMatrix();
	glPushMatrix();
	glTranslatef(-(0.7*FONTSCALE)/2,0.8,0.0);
	glColor3f(1,1,1);
	glBegin(GL_LINE_STRIP);
	{
		glVertex3f(0.0,0.0,0.0);
		glVertex3f(0.0,1.0*FONTSCALE,0.0);
		glVertex3f(0.7*FONTSCALE,0.0,0.0);
		glVertex3f(0.7*FONTSCALE,1.0*FONTSCALE,0.0);
	}
	glEnd();
	glPopMatrix();
}

- (void) drawFan
{
	int i;
	float x,y,hyp;
	
	glClearColor(0, 0, 0, 0);
	
	glPushMatrix();
	glLineWidth(3.0);
	
	if(type == SenseProfileLaser) glRotatef(270, 0.0, 0.0, 1.0);
	glTranslatef(0.0,0.8,0.0);
	glColor3f(1,1,1);
	glBegin(GL_LINES);
	{
		glVertex3f(0.0,0.0,0.0);
		glVertex3f(0.0,1.0*FONTSCALE,0.0);
		glVertex3f(-0.25*FONTSCALE,0.75*FONTSCALE,0.0);
		glVertex3f(0.0,1.0*FONTSCALE,0.0);
		glVertex3f(0.25*FONTSCALE,0.75*FONTSCALE,0.0);
		glVertex3f(0.0,1.0*FONTSCALE,0.0);
	}
	glEnd();
	glPopMatrix();
	
	glPushMatrix();
	glRotatef(displayAngle, 0.0, 0.0, 1.0);
	if(type == SenseProfileLaser) glTranslatef(0.8,-0.8, 0.0);
	else if(type == SenseBodyLaser) 
	{	
		glTranslatef(0.0, -0.6, 0.0);
		glRotatef(180, 0.0, 1.0, 0.0);
	}
	else glTranslatef(0.0,-0.6,0.0);
	
	glColor3f(1.0,1.0,1.0);
	glBegin(GL_LINES);
	{
		for(i=0;i<scanner->rayCount;i++)
		{
			hyp = scale*(float)scanner->rangeData[i];
			x = (hyp/4095.0)*cos(scanner->alpha+(float)i*scanner->dAlpha);
			y = (hyp/4095.0)*sin(scanner->alpha+(float)i*scanner->dAlpha);
			glVertex3f(x,y,0.0);
			glVertex3f(0.0,0.0,0.0);
		}
	}
	glEnd();
	glPopMatrix();
}

- (void) drawBars
{
	int i;
	float x;
	
	glClearColor(0, 0, 0, 0);
	
	if(type == SenseProfileHeight) getProfileHeights(scanner, heights);
	glPushMatrix();
	glLineWidth(3.0);
	glColor3f(1,1,1);
	glBegin(GL_LINES);
	{
		glVertex3f(-1.0,0.0,0.0);
		glVertex3f(1.0,0.0,0.0);
	}
	glEnd();
	
	// draw green line on Profile view to indicate Wheel Height
	glLineWidth(1.0);
	glColor3f(0.0, 1.0, 0.0);
	glBegin(GL_LINES);
	{
		glVertex3f(-1.0,0.4,0.0);
		glVertex3f(1.0,0.4,0.0);
	}
	glEnd();

	glLineWidth(1.0);
	glBegin(GL_LINE_STRIP);
	for(i=0;i<scanner->rayCount-2;i++){
		if(heights[i] >= 0){
			glColor3f(1, 1, 1);
			if(heights[i] > 0.5)heights[i]=0.5;
		}
		else {
			glColor3f(1, 1, 0);
			if(heights[i] < -0.5) heights[i]= -0.5;
		}
		
		x = tan(rk.PROFILEINCR*(float)i);
		x = 0.9*(x/tan(rk.PROFILEINCR*(float)scanner->rayCount/2.0))-1.0;

		heightPosition[i] = x;
		glVertex3f(heightPosition[i],2.0*heights[i],0.0);
	}
	glEnd();
	glPopMatrix();
}

- (void) drawLineProfile
{
	int i;
	float x;
	float halfAngle = (scanner->rayCount-1.0)/2.0;
	
	if(type == SensePanelHeight) getPanelHeights(scanner, heights);
	glClearColor(0, 0, 0, 0);
	
	glPushMatrix();
	glLineWidth(3.0);
	glColor3f(1,1,1);
	glBegin(GL_LINES);
	{
		glVertex3f(-1.0,0.0,0.0);
		glVertex3f(1.0,0.0,0.0);
	}
	glEnd();
	glLineWidth(1.0);
	glPointSize(4.0);
	
	glBegin(GL_LINE_STRIP);
	for(i=0;i<scanner->rayCount;i++){
		if(heights[i] >= 0)glColor3f(1, 1, 1); //white
		else glColor3f(1, 1, 0); // yellow

		x = tan(rk.PANELINCR*fabs((float)i-halfAngle));
		x = 0.9*(x/tan(rk.PANELINCR*halfAngle));
		if((i-halfAngle) > 0) x=-x;
		heightPosition[i] = x;
		glVertex3f(heightPosition[i],heights[i],0.0);
	}
	glEnd();
	
	glBegin(GL_POINTS);
	for(i=0;i<scanner->rayCount;i++){
		if(heights[i] >= 0) glColor3f(1, 1, 1);
		else glColor3f(1, 1, 0);
		glVertex3f(heightPosition[i],heights[i],0.0);
	}
	glEnd();
	
	glPopMatrix();
}

- (void)refresh
{
	if(heights) free(heights);
	heights = malloc(sizeof(float)*scanner->rayCount);
	if(heightPosition) free(heightPosition);
	heightPosition = malloc(sizeof(float)*scanner->rayCount);
}
@end
